#include "jolt_physics_direct_body_state_3d.hpp"

#include "objects/jolt_body_impl_3d.hpp"
#include "spaces/jolt_physics_direct_space_state_3d.hpp"
#include "spaces/jolt_space_3d.hpp"

JoltPhysicsDirectBodyState3DExtension::JoltPhysicsDirectBodyState3DExtension(JoltBodyImpl3D* p_body)
	: body(p_body) { }

void JoltPhysicsDirectBodyState3DExtension::_bind_methods() {
	BIND_VIRTUAL_METHOD(JoltPhysicsDirectBodyState3DExtension, _set_collision_layer);
	BIND_VIRTUAL_METHOD(JoltPhysicsDirectBodyState3DExtension, _get_collision_layer);
	BIND_VIRTUAL_METHOD(JoltPhysicsDirectBodyState3DExtension, _set_collision_mask);
	BIND_VIRTUAL_METHOD(JoltPhysicsDirectBodyState3DExtension, _get_collision_mask);
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_total_gravity() const {
	return body->get_gravity();
}

real_t JoltPhysicsDirectBodyState3DExtension::_get_total_angular_damp() const {
	return (real_t)body->get_total_angular_damp();
}

real_t JoltPhysicsDirectBodyState3DExtension::_get_total_linear_damp() const {
	return (real_t)body->get_total_linear_damp();
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_center_of_mass() const {
	return body->get_center_of_mass_relative();
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_center_of_mass_local() const {
	return body->get_center_of_mass_local();
}

Basis JoltPhysicsDirectBodyState3DExtension::_get_principal_inertia_axes() const {
	return body->get_principal_inertia_axes();
}

real_t JoltPhysicsDirectBodyState3DExtension::_get_inverse_mass() const {
	return real_t(1.0 / body->get_mass());
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_inverse_inertia() const {
	return body->get_inverse_inertia();
}

Basis JoltPhysicsDirectBodyState3DExtension::_get_inverse_inertia_tensor() const {
	return body->get_inverse_inertia_tensor();
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_linear_velocity() const {
	return body->get_linear_velocity();
}

void JoltPhysicsDirectBodyState3DExtension::_set_linear_velocity(const Vector3& p_velocity) {
	body->set_linear_velocity(p_velocity);
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_angular_velocity() const {
	return body->get_angular_velocity();
}

void JoltPhysicsDirectBodyState3DExtension::_set_angular_velocity(const Vector3& p_velocity) {
	body->set_angular_velocity(p_velocity);
}

void JoltPhysicsDirectBodyState3DExtension::_set_transform(const Transform3D& p_transform) {
	body->set_transform(p_transform);
}

Transform3D JoltPhysicsDirectBodyState3DExtension::_get_transform() const {
	return body->get_transform_scaled();
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_velocity_at_local_position(
	const Vector3& p_local_position
) const {
	return body->get_velocity_at_position(body->get_position() + p_local_position);
}

void JoltPhysicsDirectBodyState3DExtension::_apply_central_impulse(const Vector3& p_impulse) {
	body->apply_central_impulse(p_impulse);
}

void JoltPhysicsDirectBodyState3DExtension::_apply_impulse(
	const Vector3& p_impulse,
	const Vector3& p_position
) {
	body->apply_impulse(p_impulse, p_position);
}

void JoltPhysicsDirectBodyState3DExtension::_apply_torque_impulse(const Vector3& p_impulse) {
	body->apply_torque_impulse(p_impulse);
}

void JoltPhysicsDirectBodyState3DExtension::_apply_central_force(const Vector3& p_force) {
	body->apply_central_force(p_force);
}

void JoltPhysicsDirectBodyState3DExtension::_apply_force(
	const Vector3& p_force,
	const Vector3& p_position
) {
	body->apply_force(p_force, p_position);
}

void JoltPhysicsDirectBodyState3DExtension::_apply_torque(const Vector3& p_torque) {
	body->apply_torque(p_torque);
}

void JoltPhysicsDirectBodyState3DExtension::_add_constant_central_force(const Vector3& p_force) {
	body->add_constant_central_force(p_force);
}

void JoltPhysicsDirectBodyState3DExtension::_add_constant_force(
	const Vector3& p_force,
	const Vector3& p_position
) {
	body->add_constant_force(p_force, p_position);
}

void JoltPhysicsDirectBodyState3DExtension::_add_constant_torque(const Vector3& p_torque) {
	body->add_constant_torque(p_torque);
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_constant_force() const {
	return body->get_constant_force();
}

void JoltPhysicsDirectBodyState3DExtension::_set_constant_force(const Vector3& p_force) {
	body->set_constant_force(p_force);
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_constant_torque() const {
	return body->get_constant_torque();
}

void JoltPhysicsDirectBodyState3DExtension::_set_constant_torque(const Vector3& p_torque) {
	body->set_constant_torque(p_torque);
}

bool JoltPhysicsDirectBodyState3DExtension::_is_sleeping() const {
	return body->is_sleeping();
}

void JoltPhysicsDirectBodyState3DExtension::_set_collision_layer(uint32_t p_layer) {
	body->set_collision_layer(p_layer);
}

uint32_t JoltPhysicsDirectBodyState3DExtension::_get_collision_layer() const {
	return body->get_collision_layer();
}

void JoltPhysicsDirectBodyState3DExtension::_set_collision_mask(uint32_t p_mask) {
	body->set_collision_mask(p_mask);
}

uint32_t JoltPhysicsDirectBodyState3DExtension::_get_collision_mask() const {
	return body->get_collision_mask();
}

void JoltPhysicsDirectBodyState3DExtension::_set_sleep_state(bool p_enabled) {
	body->set_is_sleeping(p_enabled);
}

int32_t JoltPhysicsDirectBodyState3DExtension::_get_contact_count() const {
	return body->get_contact_count();
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_contact_local_position(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).position;
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_contact_local_normal(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).normal;
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_contact_impulse(int32_t p_contact_idx) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).impulse;
}

int32_t JoltPhysicsDirectBodyState3DExtension::_get_contact_local_shape(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).shape_index;
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_contact_local_velocity_at_position(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).velocity;
}

RID JoltPhysicsDirectBodyState3DExtension::_get_contact_collider(int32_t p_contact_idx) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).collider_rid;
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_contact_collider_position(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).collider_position;
}

uint64_t JoltPhysicsDirectBodyState3DExtension::_get_contact_collider_id(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).collider_id;
}

Object* JoltPhysicsDirectBodyState3DExtension::_get_contact_collider_object(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return ObjectDB::get_instance(body->get_contact(p_contact_idx).collider_id);
}

int32_t JoltPhysicsDirectBodyState3DExtension::_get_contact_collider_shape(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).collider_shape_index;
}

Vector3 JoltPhysicsDirectBodyState3DExtension::_get_contact_collider_velocity_at_position(
	int32_t p_contact_idx
) const {
	ERR_FAIL_INDEX_D(p_contact_idx, body->get_contact_count());
	return body->get_contact(p_contact_idx).collider_velocity;
}

real_t JoltPhysicsDirectBodyState3DExtension::_get_step() const {
	return (real_t)body->get_space()->get_last_step();
}

void JoltPhysicsDirectBodyState3DExtension::_integrate_forces() {
	const auto step = (float)_get_step();

	Vector3 linear_velocity = _get_linear_velocity();
	Vector3 angular_velocity = _get_angular_velocity();

	linear_velocity *= MAX(1.0f - (float)_get_total_linear_damp() * step, 0.0f);
	angular_velocity *= MAX(1.0f - (float)_get_total_angular_damp() * step, 0.0f);

	linear_velocity += _get_total_gravity() * step;

	_set_linear_velocity(linear_velocity);
	_set_angular_velocity(angular_velocity);
}

PhysicsDirectSpaceState3D* JoltPhysicsDirectBodyState3DExtension::_get_space_state() {
	return body->get_space()->get_direct_state();
}
